/*
 * lowpass_filter.c
 *
 *  Created on: 2021-09-27
 *      Author: chenss
 */

#include "lowpass_filter.h"

void LowPassFilter_Init(LowPassFilter_t *lpf, float Tf)
{
    assert_param(lpf != NULL);

    lpf->Tf = Tf;
    lpf->y_prev = 0;
    lpf->timestamp_prev = Tick_Microseconds();
}

float LowPassFilter_Operate(LowPassFilter_t *lpf, float x)
{
    Tick_t timestamp = Tick_Microseconds();
    float dt = (timestamp - lpf->timestamp_prev) * 1e-6f;

    if(dt < 0.0f)
    {
        dt = 1e-3f;
    }
    else if(dt > 0.3f)
    {
        lpf->y_prev = x;
        lpf->timestamp_prev = timestamp;
        return x;
    }

    float alpha = lpf->Tf / (lpf->Tf + dt);
    float y = alpha * lpf->y_prev + (1.0f - alpha) * x;
    lpf->y_prev = y;
    lpf->timestamp_prev = timestamp;
    return y;
}
